#include "paths_routing_parameters.h"
#include <iostream>
#include <string>
#include <vector>

Paths_Routing_Parameters::Paths_Routing_Parameters()
{
    paths_scommon_par = YAML::LoadFile("/home/x/path_routing_ws/src/global_smoother/params/common_parameters.yaml");

    vehicle_width  = paths_scommon_par["vehicle_width"].as<double>();
    vehicle_length  = paths_scommon_par["vehicle_length"].as<double>();

    // frenet采样规划参数
    Max_speed = paths_scommon_par["Max_speed"].as<double>();
    Max_accel = paths_scommon_par["Max_accel"].as<double>();
    Max_curvature = paths_scommon_par["Max_curvature"].as<double>();
    left_d = paths_scommon_par["left_d"].as<double>();
    right_d = paths_scommon_par["right_d"].as<double>();
    dDelta = paths_scommon_par["dDelta"].as<double>();
    tDelta = paths_scommon_par["tDelta"].as<double>();
    Maxt = paths_scommon_par["Maxt"].as<double>();
    Mint = paths_scommon_par["Mint"].as<double>();
    D_t_s = paths_scommon_par["D_t_s"].as<double>();   //目标速度采样间隔 [m/s]
    N_s_sample = paths_scommon_par["N_s_sample"].as<double>();    // 目标速度的采样数量
    D_s = paths_scommon_par["D_s"].as<double>();        //目标位置采样间隔 [m]
    N_s_position = paths_scommon_par["N_s_position"].as<double>();  // 目标位置的采样数量
    //横向权重
    wlat_d = paths_scommon_par["wlat_d"].as<double>();  //横向偏差
    wlat_v = paths_scommon_par["wlat_v"].as<double>(); //横向速度
    wlat_a = paths_scommon_par["wlat_a"].as<double>(); //横向加速度
    wlat_j = paths_scommon_par["wlat_j"].as<double>(); //横向jerk（舒适性，即加速度导数）
    //纵向权重
    wlon_a = paths_scommon_par["wlon_a"].as<double>(); //纵向加速度
    wlon_j = paths_scommon_par["wlon_j"].as<double>();  //纵向jerk
    wlon_v = paths_scommon_par["wlon_v"].as<double>(); //纵向速度
    wlon_t = paths_scommon_par["wlon_t"].as<double>(); //纵向时间（也就路径越短越好）
    //总的
    wlat_score = paths_scommon_par["wlat_score"].as<double>(); // 横向权重得分
    wlon_score = paths_scommon_par["wlon_score"].as<double>(); //纵向权重得分

    target_speed = paths_scommon_par["target_speed"].as<double>(); 
    initial_vs = paths_scommon_par["initial_vs"].as<double>(); 


}

Paths_Routing_Parameters::~Paths_Routing_Parameters()
{
}
